#ifndef _SCENES_JUDGE_H_
#define _SCENES_JUDGE_H_

#include <rclcpp/rclcpp.hpp>
#include <tinyxml2.h>
#include <stdio.h>
#include <unistd.h>
#include <utility>
#include <sys/types.h>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <unordered_map>
#include <iostream>
#include <vector>
#include <mutex>

using namespace std;
using namespace tinyxml2;

#define EVENT_STRING "9527"
#define EVENT_INT 9527

#define BIT(x) (((uint64_t)1) << (x))
#define TestBit(x, i) ((x & BIT(i)) != 0)
#define SET_BIT(x, n) (x | ((uint64_t)(1) << n))
#define CLEAR_BIT(x, n) (x & ~((uint64_t)(1) << n))

namespace AgvObstaAvoid {

class ScenesJudge 
{
public:
    enum AgvWorkMode {
        manual = 0,
        automatic = 2,
    };

    enum AgvTrajType {
        TRAJ_LINE =  0,
        TRAJ_BEZIER = 1,
        TRAJ_SIDE =  2,
        TRAJ_SPIN =  3,
        TRAJ_BSPLINE = 4,
    };

    struct cEquip {
        int name;
        string type;
        int area;
        bool warnenable;
        bool slowenable;
        bool stopenable;
    };
    
    struct cPlan {
        string name;
        int default_state;
        int velocity_state;
        int load_state;
        int traj_state;
        int bit_state;
        vector<cEquip> equiplist;
    };
    
    struct AgvState {
        int velocity;
        int load;
        int traj_type;
        int bits;

        AgvState() {
            velocity = 0;
            load = 0;
            traj_type = 0;
            bits = 0;
        }

        bool operator==(const AgvState& one) {
            return ((velocity == one.velocity) && (load == one.load) && 
                    (traj_type == one.traj_type) && (bits == one.bits));
        }

        bool operator!=(const AgvState& one) {
            return ((velocity != one.velocity) || (load != one.load) || 
                    (traj_type != one.traj_type) || (bits != one.bits));
        }
    };
    
    struct EquipDispatch {
        vector<ScenesJudge::cEquip> scan_equip;
        vector<ScenesJudge::cEquip> dido_equip;
        vector<ScenesJudge::cEquip> came_equip;
    };

    ScenesJudge();
    ~ScenesJudge() = default;
    
    // void init();
    bool ScenesReadXml(XMLElement* element);
    bool ShowScenesPlan(cPlan* plan_item);
    bool ScenesSwitch(const string planname);
    EquipDispatch PlanEquipDistribute();
    bool AgvStatePolling();

    void OdomCallback(const agv_msgs::msg::OdometryLite::SharedPtr odom_msg);
    void LoadCallback(const agv_msgs::msg::PeriphStatus::SharedPtr load_msg);
    void PathCallback(const agv_msgs::msg::AgvAutoStatus::SharedPtr path_msg);

    AgvState agv_state;         // agv状态
    AgvState agv_state_main;    // agv上一次的状态
    int workstate;
    string scenes_name;
    unordered_map<string, cPlan> plan_list;
    vector<ScenesJudge::cEquip> scan_equip_manual;
    vector<ScenesJudge::cEquip> dido_equip_manual;
    vector<ScenesJudge::cEquip> came_equip_manual;   

private:
    string scenes_from_map;
    mutex scenes_mutex_;

    // ROS2 subscribers
    rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odom_sub_;
    rclcpp::Subscription<agv_msgs::msg::PeriphStatus>::SharedPtr load_sub_;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr path_sub_;

    AgvState GetAgvStateFromPlan(const cPlan& plan);
    void ShowAgvState(const AgvState& state);
    bool AgvBitJudge(AgvState agv_state, AgvState state_plan);
    bool AgvStateJudge(AgvState agv_state, AgvState state_plan);
};

} // namespace AgvObstaAvoid end
#endif